# -*- coding: utf-8 -*-
"""
Created on Mon Oct  2 11:34:34 2022

@author: wulong
"""
import numpy as np
from casadi import *

from Real_system import *
from Weight_eval import *
from time import time

from Day_solver import *
from Slow_EMPC_solver import *
from Fast_1_EMPC_solver import *
from Fast_2_EMPC_solver import *
from Fast_3_EMPC_solver import *

from Transfer_opt_xs_h import *
from Transfer_opt_xs_s import *
from Transfer_seq_f import *

# In[0] Inputs' constraints factors
# define acceptable range
fc_dk_u = 0.2

fc_nb_u = 0.015
fc_it_u = 0.015
fc_pr_u = 0.025

# define individual coefficients for each fast system
co_nb_uf1 = np.array([8, 20])
co_it_uf1 = np.array([8, 8])
co_pr_uf1 = np.array([8, 12])

co_nb_uf2 = 4
co_it_uf2 = 1
co_pr_uf2 = 4

co_nb_uf3 = 4
co_it_uf3 = 1
co_pr_uf3 = 4

# In[1] Common parameters
pf = 0.2
pmg = 80/(3.6e6)
Cmp_co = 1.5
Pen_co = 1.5

# States and inputs bounds
Uc_bnd_lo = np.array([0.00055, 0.002, 1.9, 20, 1.3, 0.2, -40])
Uc_bnd_up = np.array([0.0045, 0.0066911, 3.5714, 110, 2.381, 1, 40])
Uz_bnd_lo = np.array([0, 0, 0, 0])
Uz_bnd_up = np.array([1, 1, 1, 1])

Delta_Uc = Uc_bnd_up - Uc_bnd_lo

# actual input's acceptable increment range between "two continuous seconds"
# used for evaluating the chage rate between two time instants
# u(k) - u(k-1)
D_Uc = Delta_Uc*fc_dk_u

X_bnd_lo = np.array([13, 0.03, 0.006, 0.095, 0.002, -75, -0.5, -4, -5,
                     27, 25, 25, -5, 0, 7,
                     -0.24, 0.1, -192, 0.1, 0, 0, 8, 18])
X_bnd_up = np.array([130, 0.4, 0.06, 1, 0.08, 5.00, 3, 1, 10,
                     44, 38, 35, 20, 22, 22,
                     0.26, 1, 211, 1, 260000, 360000, 18, 26])

Y_bnd_lo = np.array([-100, 16])
Y_bnd_up = np.array([200, 28])

# Scalized weight coefficients
Wet = Weight_EMPC(Uc_bnd_lo, Uc_bnd_up, X_bnd_lo, X_bnd_up)

wet_eco = Wet[0]
wet_ye = Wet[1]
wet_y1 = Wet[2]
wet_y2 = Wet[3]
wet_xs = Wet[4]
wet_xf1 = Wet[5]
wet_xf2 = Wet[6]
wet_xf3 = Wet[7]
wet_uf1 = Wet[8]
wet_uf2 = Wet[9]
wet_uf3 = Wet[10]

# Zone parameters
delta_tbr = 0.5

# External data
# All of data are without initial points.
# Load hourly and 1 min data, [ta, Sra, Pd, Qo, tbr, pse, rxi] 4+3=7
data_h = np.loadtxt('data_dayahead.txt', delimiter = ', ')
data_r_temp_0 = np.loadtxt('data_real.txt', delimiter = ', ')

# Repeat real data for each sampling time
data_r_temp_1 = np.repeat(data_r_temp_0, Delta_s/Delta_r, axis=0)

# Tiled data for prediction in receding horizon
data_r = np.tile(data_r_temp_1, (2,1))

# Extract disturbance, output setpoint and price, regeulation factor
D_h = data_h[:,0:4]
D_r = data_r[:,0:4]

Y2_stp_lo_h = data_h[:,4] - delta_tbr*0.7
Y2_stp_up_h = data_h[:,4] + delta_tbr*0.7

Y2_stp_lo_r = data_r[:,4] - delta_tbr
Y2_stp_up_r = data_r[:,4] + delta_tbr

pse_h = data_h[:,5]
pse_r = data_r[:,5]

pcm_h = Cmp_co*pse_h
pcm_r = Cmp_co*pse_r

ppn_h = Pen_co*pse_h
ppn_r = Pen_co*pse_r

rxi_h = data_h[:,6]
rxi_r = data_r[:,6]

rxi_as_h = np.abs(rxi_h)
rxi_as_r = np.abs(rxi_r)

# In[2] Day-ahead Optimizaton
pre_horz_h = 24

alpha_h = wet_eco*1
alpha_ye_h = wet_ye*np.array([1,1])

seed_h = int(Delta_h/Delta_r)
seed_h_s = int(Delta_h/Delta_s)
seed_h_f = int(Delta_h/Delta_f)

X_bnd_lo_h = np.array([X_bnd_lo[16], X_bnd_lo[18], X_bnd_lo[22]])
X_bnd_up_h = np.array([X_bnd_up[16], X_bnd_up[18], X_bnd_up[22]])

Uc_bnd_lo_h = np.array([0, 0, 0, Uc_bnd_lo[-2], Uc_bnd_lo[-1]])
Uc_bnd_up_h = np.delete(Uc_bnd_up, [2,4])

Uc_lbg_h = np.delete(Uc_bnd_lo, [2,4])
Uc_ubg_h = np.delete(Uc_bnd_up, [2,4])

Uz_bnd_lo_h = Uz_bnd_lo
Uz_bnd_up_h = Uz_bnd_up

Y_bnd_lo_h = Y_bnd_lo
Y_bnd_up_h = Y_bnd_up

# In[3] S-EMPC
pre_horz_s = 12

alpha_s = np.array([wet_y1, wet_y2, wet_eco, 
                    wet_xs[0], wet_xs[1]])*np.array([3, 7, 0.2, 1, 1])

seed_s = int(Delta_s/Delta_r)
seed_s_f = int(Delta_s/Delta_f)

X_bnd_lo_s = np.append(np.insert(X_bnd_lo[18:21], 0, X_bnd_lo[16]), X_bnd_lo[22])
X_bnd_up_s = np.append(np.insert(X_bnd_up[18:21], 0, X_bnd_up[16]), X_bnd_up[22])

Xf_bnd_lo_s = np.delete(X_bnd_lo, [16, 18, 19, 20, 22])
Xf_bnd_up_s = np.delete(X_bnd_up, [16, 18, 19, 20, 22])

Uc_bnd_lo_s = np.append(Uc_bnd_lo[2], Uc_bnd_lo[4:6])
Uc_bnd_up_s = np.append(Uc_bnd_up[2], Uc_bnd_up[4:6])

Ucf_bnd_lo_s = np.delete(Uc_bnd_lo, [2, 4, 5])
Ucf_bnd_up_s = np.delete(Uc_bnd_up, [2, 4, 5])

Delta_Uc_s = np.append(D_Uc[2], D_Uc[4:6])
Delta_Ucf_s = np.delete(D_Uc, [2, 4, 5])

Y_bnd_lo_s = Y_bnd_lo
Y_bnd_up_s = Y_bnd_up

# External data
D_s = D_r[::seed_s, :]

Y2_stp_lo_s = Y2_stp_lo_r[::seed_s]
Y2_stp_up_s = Y2_stp_up_r[::seed_s]

pse_s = pse_r[::seed_s]

pcm_s = pcm_r[::seed_s]
ppn_s = ppn_r[::seed_s]
rxi_s = rxi_r[::seed_s]
rxi_as_s = rxi_as_r[::seed_s]

# In[4] General parameters for fast empcs
seed_f = int(Delta_f/Delta_r)

# Ensure fast empcs stability

# the acceptable neighbourhood range of 
# slow optimal values at a time instant
# u(k) - u(k)*
Uc_nb = Delta_Uc*fc_nb_u

Uc_nb_f1 = np.append(Uc_nb[0], Uc_nb[6])*co_nb_uf1
Uc_nb_f2 = Uc_nb[1]*co_nb_uf2
Uc_nb_f3 = Uc_nb[3]*co_nb_uf3

# the acceptable change rate of fast inputs between two 
# continuous iterations at the same time instant
# u(k)^{c} - u(k)^{c-1}
It_d_Uc = Delta_Uc*fc_it_u

It_d_Uc_f1 = np.append(It_d_Uc[0], It_d_Uc[6])*co_it_uf1
It_d_Uc_f2 = It_d_Uc[1]*co_it_uf2
It_d_Uc_f3 = It_d_Uc[3]*co_it_uf3

# the acceptable change rate of fast inputs between prediction
# at last time instant and actual value at the current instant
# u(k) - u(k/k-1)
pre_d_Uc = Delta_Uc*fc_pr_u

pre_d_Uc_f1 = np.append(pre_d_Uc[0], pre_d_Uc[6])*co_pr_uf1
pre_d_Uc_f2 = pre_d_Uc[1]*co_pr_uf2
pre_d_Uc_f3 = pre_d_Uc[3]*co_pr_uf3

# Iteration conditions
iterate_num = 12
error_tol = 5e-2 # relative error tolerance

D_f = D_r[::seed_f, :]
pse_f = pse_r[::seed_f]

pcm_f = pcm_r[::seed_f]
ppn_f = ppn_r[::seed_f]
rxi_f = rxi_r[::seed_f]
rxi_as_f = rxi_as_r[::seed_f]

# In[5] F-EMPC 1
pre_horz_f1 = 10

alpha_f1 = np.array([wet_y1, wet_eco])*np.array([8, 0.001])
alpha_x_f1 = np.diag(wet_xf1)*4
alpha_u_f1 = np.diag(wet_uf1)*np.array([1,1])
alpha_ue_f1 = np.diag(wet_uf1)*np.array([1,1])

X_bnd_lo_f1 = np.append(np.append(X_bnd_lo[:5], X_bnd_lo[15]), X_bnd_lo[17])
X_bnd_up_f1 = np.append(np.append(X_bnd_up[:5], X_bnd_up[15]), X_bnd_up[17])

Uc_bnd_lo_f1 = np.append(Uc_bnd_lo[0], Uc_bnd_lo[6])
Uc_bnd_up_f1 = np.append(Uc_bnd_up[0], Uc_bnd_up[6])

Delta_Uc_f1 = np.append(D_Uc[0], D_Uc[6])

Y_bnd_lo_f1 = Y_bnd_lo[0]
Y_bnd_up_f1 = Y_bnd_up[0]

# In[6] F-EMPC 2
pre_horz_f2 = 12

alpha_f2 = np.array([wet_y1, wet_eco])*np.array([0.001, 0.001])
alpha_x_f2 = np.diag(wet_xf2)*4
alpha_u_f2 = np.diag(wet_uf2.reshape(1,-1))*1
alpha_ue_f2 = np.diag(wet_uf2.reshape(1,-1))*1

X_bnd_lo_f2 = np.append(np.append(X_bnd_lo[5:9], X_bnd_lo[14]), X_bnd_lo[21])
X_bnd_up_f2 = np.append(np.append(X_bnd_up[5:9], X_bnd_up[14]), X_bnd_up[21])

Uc_bnd_lo_f2 = Uc_bnd_lo[1]
Uc_bnd_up_f2 = Uc_bnd_up[1]

Delta_Uc_f2 = D_Uc[1]

Y_bnd_lo_f2 = Y_bnd_lo[0]
Y_bnd_up_f2 = Y_bnd_up[0]

# In[7] F-EMPC 3
pre_horz_f3 = 10

alpha_f3 = np.array([wet_y1, wet_eco])*np.array([0.001, 0.001])
alpha_x_f3 = np.diag(wet_xf3)*4
alpha_u_f3 = np.diag(wet_uf3.reshape(1,-1))*1
alpha_ue_f3 = np.diag(wet_uf3.reshape(1,-1))*1

X_bnd_lo_f3 = X_bnd_lo[9:14]
X_bnd_up_f3 = X_bnd_up[9:14]

Uc_bnd_lo_f3 = Uc_bnd_lo[3]
Uc_bnd_up_f3 = Uc_bnd_up[3]

Delta_Uc_f3 = D_Uc[3]

Y_bnd_lo_f3 = Y_bnd_lo[0]
Y_bnd_up_f3 = Y_bnd_up[0]

# In[8] Simulate things
Simtime = 3600*24
Nsim = int(Simtime/Delta_r)

# Initializatioin
pre_horz_f = np.array([pre_horz_f1, pre_horz_f2, pre_horz_f3])
J1_0 = 1e6
J2_0 = 1e6
J3_0 = 1e6
tag = 1

C_soc_0 = 0.11
C_sot_0 = 0.11
C_stc_0 = C_sot_0*2e4*7
C_sth_0 = (1 - C_sot_0)*2e4*12

X_int = np.array([112.952, 0.28125, 0.052831, 0.800712, 0.066726, 0, 0, 0, 0,
                  39.9997, 34.6358, 30.5001, 2.00017, 6.23489, 9.50001,
                  0, C_soc_0, 0, C_sot_0, C_stc_0, C_sth_0, 12, 22])
Y_int = np.array([0, 22]) # Assume that Pd is 93.5.
U_int = np.array([0.0045, 0.0066911, 3.5714, 110, 2.381, 0, 0])
Z_int = np.array([1, 1, 1, 1])

# Initial guess
X_guess_h = np.tile(np.array([[X_int[16], X_int[18], X_int[22]]]), (pre_horz_h,1))
Uc_guess_h = np.tile(np.delete(U_int, [2,4]).reshape(1,-1), (pre_horz_h,1))
Uz_guess_h = np.tile(Z_int.reshape(1,-1), (pre_horz_h,1))
Y_guess_h = np.tile(Y_int.reshape(1,-1), (pre_horz_h,1))

Xs_guess_s_temp = np.append(np.insert(X_int[18:21], 0, X_int[16]), X_int[22])
Xf_guess_s_temp = np.delete(X_int, [16, 18, 19, 20, 22])

Xs_guess_s = np.tile(Xs_guess_s_temp.reshape(1,-1), (pre_horz_s,1))
Xf_guess_s = np.tile(Xf_guess_s_temp.reshape(1,-1), (pre_horz_s,1))

Us_guess_s = np.tile(np.append(U_int[2], U_int[4:6]).reshape(1,-1), (pre_horz_s,1))
Uf_guess_s = np.tile(np.delete(U_int, [2, 4, 5]).reshape(1,-1), (pre_horz_s,1))

Ys_guess_s = np.tile(Y_int.reshape(1,-1), (pre_horz_s,1))
Yspe_guess_s = np.tile(Y_int[1].reshape(1,-1), (pre_horz_s,1))

past_Uz_s = Z_int
past_Uz_f = Z_int
past_Uc_s = np.append(U_int[2], U_int[4:6])
past_Ucf_s = np.delete(U_int, [2, 4, 5])
past_Uc_f1 = np.append(U_int[0], U_int[6])
past_Uc_f2 = U_int[1]
past_Uc_f3 = U_int[3]

# Data should be recorded.
# We don't record external data here.
# The following data include initial/last points.
X = np.zeros((Nsim+1, Nx_r))
Y = np.zeros((Nsim+1, Ny_r))
U = np.zeros((Nsim+1, Nuc_r))
Z = np.zeros((Nsim+1, Nuz_r))

# The following data DO NOT include initial/last points.
# And record with individual sampling step.
Xf_stp_s = np.zeros((int(np.ceil(Nsim/seed_s)), Nx_f))
Uf_stp_s = np.zeros((int(np.ceil(Nsim/seed_s)), Nuc_f))
It_num_f = np.zeros(int(np.ceil(Nsim/seed_f)))

# Initial points are left just for plot.
# Y[0] will be given when simulation finished.
X[0,:] = X_int

time_start_sim = time()

# In[9] Day-ahead optimization
time_start_h = time()

X_int_h = np.array([X_int[16], X_int[18], X_int[22]])

Opt_h = D_OPT(X_int_h, X_guess_h, X_bnd_lo_h, X_bnd_up_h,
              Uc_guess_h, Uc_bnd_lo_h, Uc_bnd_up_h,
              Uc_lbg_h, Uc_ubg_h,
              Uz_guess_h, Uz_bnd_lo_h, Uz_bnd_up_h,
              D_h,
              Y_guess_h,
              Y2_stp_lo_h, Y2_stp_up_h,
              Y_bnd_lo_h, Y_bnd_up_h,
              alpha_h, alpha_ye_h, pre_horz_h,
              pf, pmg, pse_h, 0,
              pcm_h, ppn_h, rxi_h, rxi_as_h)

# Get optimal guess sequence
Uc_guess_h = Opt_h[0].reshape(pre_horz_h, -1)
Uz_guess_h = Opt_h[1].reshape(pre_horz_h, -1)
X_guess_h = Opt_h[2].reshape(pre_horz_h, -1)
Y_guess_h = Opt_h[3].reshape(pre_horz_h, -1)
Yeb_guess_h = Opt_h[5].reshape(pre_horz_h, -1)

# Get data that should be sent
Yeb_r_temp = np.repeat(Yeb_guess_h, seed_h, axis=0)
Yeb_r = np.tile(Yeb_r_temp, (2,1))

Yeb_s = Yeb_r[::seed_s, 0]
Yeb_f = Yeb_r[::seed_f, 0]

Xs_stp_h = tras_opt_xs_h(X_int_h, X_guess_h, seed_h_s)

Uz_r_temp = np.repeat(Uz_guess_h, seed_h, axis=0)
Uz_r = np.tile(Uz_r_temp, (2,1))

Uz_s = Uz_r[::seed_s, :]
Uz_f = Uz_r[::seed_f, :]

# Record data
np.savetxt("P1_Yeb_stp_h.txt", Yeb_guess_h, fmt='%.10e', delimiter = ', ')
np.savetxt("P1_Xs_stp_h.txt", X_guess_h, fmt='%.10e', delimiter = ', ')
np.savetxt("P1_Uz_stp_h.txt", Uz_guess_h, fmt='%.10e', delimiter = ', ')

time_end_h = time()
print('------------------------------------------------')
print('Day-ahead time: '+ str(time_end_h - time_start_h))
print('------------------------------------------------')

# In[10] Distributed EMPC simulation

for i in range(Nsim):
    
    # Get binary inputs at i
    Uzk = Uz_r[i,:]
    
    # Solve S-EMPC
    if i % seed_s == 0:
        
        time_start_s = time()
        
        i_s = int(i/seed_s)
        
        X_int_s = np.append(np.insert(X_int[18:21], 0, X_int[16]), X_int[22])
        Xf_int_s = np.delete(X_int, [16, 18, 19, 20, 22])
        
        Opt_s = S_EMPC(X_int_s, Xs_guess_s, X_bnd_lo_s, X_bnd_up_s,
                       Xf_guess_s, Xf_bnd_lo_s, Xf_bnd_up_s,
                       Us_guess_s, Uc_bnd_lo_s, Uc_bnd_up_s,
                       Uf_guess_s, Ucf_bnd_lo_s, Ucf_bnd_up_s,
                       Uz_s,
                       D_s,
                       Ys_guess_s,
                       Yspe_guess_s,
                       Y2_stp_lo_s, Y2_stp_up_s, 
                       Y_bnd_lo_s, Y_bnd_up_s, Xs_stp_h,
                       alpha_s, pre_horz_s, pf, pmg, pse_s,
                       i_s, Xf_int_s, 
                       i, Delta_Uc_s, Delta_Ucf_s, past_Uc_s, past_Ucf_s, past_Uz_s,
                       pcm_s, ppn_s, rxi_s, rxi_as_s, Yeb_s)
        
        # Get optimal guess sequence
        Us_guess_s = Opt_s[0].reshape(pre_horz_s, -1)
        Uf_guess_s = Opt_s[1].reshape(pre_horz_s, -1)
        Xs_guess_s = Opt_s[2].reshape(pre_horz_s, -1)
        Xf_guess_s = Opt_s[3].reshape(pre_horz_s, -1)
        Ys_guess_s = Opt_s[4].reshape(pre_horz_s, -1)
        Yspe_guess_s = Opt_s[5].reshape(pre_horz_s, -1)
        
        # Get slow inputs
        Usk = Us_guess_s[0,:]
        past_Uc_s = Usk
        past_Uz_s = Uzk
        
        # Get data that should be sent
        Xs_f_opt_s = tras_opt_xs_s(X_int_s, Xs_guess_s, seed_s_f)
        Us_f_opt_s = np.repeat(Us_guess_s[:2,:], seed_s_f, axis=0)
        Xf_stp_opt_s = np.repeat(Xf_guess_s[:2,:], seed_s_f, axis=0)
        Uf_stp_opt_s = np.repeat(Uf_guess_s[:2,:], seed_s_f, axis=0)
        Y1_f_opt_s = np.repeat(Ys_guess_s[:2,0].reshape(-1,1), seed_s_f, axis=0)
        
        Xf1_stp_opt_s = np.hstack((Xf_stp_opt_s[:,:5], Xf_stp_opt_s[:,15:17]))
        Xf2_stp_opt_s = np.hstack((np.hstack((Xf_stp_opt_s[:,5:9], Xf_stp_opt_s[:,14].reshape(-1,1))),
                                   Xf_stp_opt_s[:,17].reshape(-1,1)))
        Xf3_stp_opt_s = Xf_stp_opt_s[:,9:14]
        
        Uf1_stp_opt_s = np.hstack((Uf_stp_opt_s[:,0].reshape(-1,1), Uf_stp_opt_s[:,3].reshape(-1,1)))
        Uf2_stp_opt_s = Uf_stp_opt_s[:,1].reshape(-1,1)
        Uf3_stp_opt_s = Uf_stp_opt_s[:,2].reshape(-1,1)
        
        # Record data
        Xf_stp_s[i_s,:] = Xf_guess_s[0,:]
        Uf_stp_s[i_s,:] = Uf_guess_s[0,:]
        
        time_end_s = time()
        print('------------------------------------------------')
        print('S-EMPC time: '+ str(time_end_s - time_start_s))
        print('------------------------------------------------')
        
        pass
    
    
    # Solve F-EMPC
    if i % seed_f == 0:
        
        time_start_f = time()
        
        i_f = int(i/seed_f)
        i_s_f = int(i_f - i_s*seed_s_f)
        
        X_int_f1 = np.append(np.append(X_int[:5], X_int[15]), X_int[17])
        X_int_f2 = np.append(np.append(X_int[5:9], X_int[14]), X_int[21])
        X_int_f3 = X_int[9:14]
        
        Uz_f_sq = tras_sq_f(Uz_f, i_f, pre_horz_f)
        Xs_f_sq = tras_sq_f(Xs_f_opt_s, i_s_f, pre_horz_f)
        Us_f_sq = tras_sq_f(Us_f_opt_s, i_s_f, pre_horz_f)
        Y1_f_sq = tras_sq_f(Y1_f_opt_s, i_s_f, pre_horz_f)
        
        Xf1_stp_sq = Xf1_stp_opt_s[i_s_f:i_s_f+pre_horz_f1,:]
        Xf2_stp_sq = Xf2_stp_opt_s[i_s_f:i_s_f+pre_horz_f2,:]
        Xf3_stp_sq = Xf3_stp_opt_s[i_s_f:i_s_f+pre_horz_f3,:]
        
        Uf1_stp_sq = Uf1_stp_opt_s[i_s_f:i_s_f+pre_horz_f1,:]
        Uf2_stp_sq = Uf2_stp_opt_s[i_s_f:i_s_f+pre_horz_f2,:]
        Uf3_stp_sq = Uf3_stp_opt_s[i_s_f:i_s_f+pre_horz_f3,:]
        
        D_f_sq = tras_sq_f(D_f, i_f, pre_horz_f)
        pse_f_sq = tras_sq_f(pse_f.reshape(-1,1), i_f, pre_horz_f)
        
        pcm_f_sq = tras_sq_f(pcm_f.reshape(-1,1), i_f, pre_horz_f)
        ppn_f_sq = tras_sq_f(ppn_f.reshape(-1,1), i_f, pre_horz_f)
        rxi_f_sq = tras_sq_f(rxi_f.reshape(-1,1), i_f, pre_horz_f)
        rxi_as_f_sq = tras_sq_f(rxi_as_f.reshape(-1,1), i_f, pre_horz_f)
        Yeb_f_sq = tras_sq_f(Yeb_f.reshape(-1,1), i_f, pre_horz_f)
        
        # initial guess in fast subsystems by using slow empc's optimal value
        Xf1_guess_s = Xf1_stp_sq
        Uf1_guess_s = Uf1_stp_sq
        Yf1_guess_s = Y1_f_sq[0]
        
        Xf2_guess_s = Xf2_stp_sq
        Uf2_guess_s = Uf2_stp_sq
        Yf2_guess_s = Y1_f_sq[1]
        
        Xf3_guess_s = Xf3_stp_sq
        Uf3_guess_s = Uf3_stp_sq
        Yf3_guess_s = Y1_f_sq[2]
        
        # just for first exchange of information in the simulation (initialization)
        if i == 0:
            
            Xf1_guess_f1 = Xf1_stp_sq
            Xf2_guess_f2 = Xf2_stp_sq
            Xf3_guess_f3 = Xf3_stp_sq
            
            Uf1_guess_f1 = Uf1_stp_sq
            Uf2_guess_f2 = Uf2_stp_sq
            Uf3_guess_f3 = Uf3_stp_sq
            
            pre_sq_Uc_f1 = Uf1_guess_f1
            pre_sq_Uc_f2 = Uf2_guess_f2
            pre_sq_Uc_f3 = Uf3_guess_f3
            
            pre_sq_Uz_f1 = Uz_f_sq[0]
            pre_sq_Uz_f2 = Uz_f_sq[1]
            pre_sq_Uz_f3 = Uz_f_sq[2]
            pass
        
        while tag <= iterate_num:
            
            Opt_f1 = F1_EMPC(X_int_f1, Xf1_guess_s, X_bnd_lo_f1, X_bnd_up_f1,
                             Xs_f_sq[0], Xf2_guess_f2, Xf3_guess_f3,
                             Us_f_sq[0], Uf2_guess_f2, Uf3_guess_f3,
                             Uf1_guess_s, Uc_bnd_lo_f1, Uc_bnd_up_f1,
                             Uz_f_sq[0],
                             D_f_sq[0],
                             Yf1_guess_s, Y_bnd_lo_f1, Y_bnd_up_f1,
                             Xf1_stp_sq, Uf1_stp_sq,
                             alpha_f1, alpha_x_f1, alpha_u_f1, pre_horz_f1,
                             pf, pmg, pse_f_sq[0],
                             tag, pre_horz_f2, pre_horz_f3, X_int_f2, X_int_f3, 
                             i, Delta_Uc_f1, past_Uc_f1, past_Uz_f, 
                             alpha_ue_f1, Uc_nb_f1, It_d_Uc_f1, Uf1_guess_f1,
                             pre_d_Uc_f1, pre_sq_Uc_f1, pre_sq_Uz_f1,
                             pcm_f_sq[0], ppn_f_sq[0], rxi_f_sq[0], 
                             rxi_as_f_sq[0], Yeb_f_sq[0])
            
            Opt_f2 = F2_EMPC(X_int_f2, Xf2_guess_s, X_bnd_lo_f2, X_bnd_up_f2,
                             Xs_f_sq[1], Xf1_guess_f1, Xf3_guess_f3,
                             Us_f_sq[1], Uf1_guess_f1, Uf3_guess_f3,
                             Uf2_guess_s, Uc_bnd_lo_f2, Uc_bnd_up_f2,
                             Uz_f_sq[1],
                             D_f_sq[1],
                             Yf2_guess_s, Y_bnd_lo_f2, Y_bnd_up_f2,
                             Xf2_stp_sq, Uf2_stp_sq,
                             alpha_f2, alpha_x_f2, alpha_u_f2, pre_horz_f2,
                             pf, pmg, pse_f_sq[1],
                             tag, pre_horz_f1, pre_horz_f3, X_int_f1, X_int_f3, 
                             i, Delta_Uc_f2, past_Uc_f2, past_Uz_f, 
                             alpha_ue_f2, Uc_nb_f2, It_d_Uc_f2, Uf2_guess_f2,
                             pre_d_Uc_f2, pre_sq_Uc_f2, pre_sq_Uz_f2,
                             pcm_f_sq[1], ppn_f_sq[1], rxi_f_sq[1], 
                             rxi_as_f_sq[1], Yeb_f_sq[1])
            
            Opt_f3 = F3_EMPC(X_int_f3, Xf3_guess_s, X_bnd_lo_f3, X_bnd_up_f3,
                             Xs_f_sq[2], Xf1_guess_f1, Xf2_guess_f2,
                             Us_f_sq[2], Uf1_guess_f1, Uf2_guess_f2,
                             Uf3_guess_s, Uc_bnd_lo_f3, Uc_bnd_up_f3,
                             Uz_f_sq[2],
                             D_f_sq[2],
                             Yf3_guess_s, Y_bnd_lo_f3, Y_bnd_up_f3,
                             Xf3_stp_sq, Uf3_stp_sq,
                             alpha_f3, alpha_x_f3, alpha_u_f3, pre_horz_f3,
                             pf, pmg, pse_f_sq[2],
                             tag, pre_horz_f1, pre_horz_f2, X_int_f1, X_int_f2, 
                             i, Delta_Uc_f3, past_Uc_f3, past_Uz_f, 
                             alpha_ue_f3, Uc_nb_f3, It_d_Uc_f3, Uf3_guess_f3,
                             pre_d_Uc_f3, pre_sq_Uc_f3, pre_sq_Uz_f3,
                             pcm_f_sq[2], ppn_f_sq[2], rxi_f_sq[2], 
                             rxi_as_f_sq[2], Yeb_f_sq[2])
            
            # Get optimal guess sequence
            Uf1_guess_f1 = Opt_f1[0][0].reshape(pre_horz_f1, -1)
            Xf1_guess_f1 = Opt_f1[0][1].reshape(pre_horz_f1, -1)
            Yf1_guess_f1 = Opt_f1[0][2].reshape(pre_horz_f1, -1)
            
            Uf2_guess_f2 = Opt_f2[0][0].reshape(pre_horz_f2, -1)
            Xf2_guess_f2 = Opt_f2[0][1].reshape(pre_horz_f2, -1)
            Yf2_guess_f2 = Opt_f2[0][2].reshape(pre_horz_f2, -1)
            
            Uf3_guess_f3 = Opt_f3[0][0].reshape(pre_horz_f3, -1)
            Xf3_guess_f3 = Opt_f3[0][1].reshape(pre_horz_f3, -1)
            Yf3_guess_f3 = Opt_f3[0][2].reshape(pre_horz_f3, -1)
            
            # Get cost evluation
            J1 = Opt_f1[1]
            re_J1 = np.abs((J1 - J1_0)/J1_0)
            
            J2 = Opt_f2[1]
            re_J2 = np.abs((J2 - J2_0)/J2_0)
            
            J3 = Opt_f3[1]
            re_J3 = np.abs((J3 - J3_0)/J3_0)
            
            # Record data
            It_num_f[i_f] = tag
            
            # Converged or not
            if re_J1 <= error_tol and re_J2 <= error_tol and re_J3 <= error_tol:
                break
            
            J1_0 = J1
            J2_0 = J2
            J3_0 = J3
            tag += 1
            
            pass
        
        # Get fast inputs
        Uf1k = Uf1_guess_f1[0,:]
        Uf2k = Uf2_guess_f2[0,:]
        Uf3k = Uf3_guess_f3[0,:]
        
        past_Uc_f1 = Uf1k
        past_Uc_f2 = Uf2k
        past_Uc_f3 = Uf3k
        past_Ucf_s = np.array([Uf1k[0], Uf2k[0], Uf3k[0], Uf1k[1]])
        past_Uz_f = Uzk
        
        pre_sq_Uc_f1 = np.delete(Uf1_guess_f1, 0, axis=0)
        pre_sq_Uc_f2 = np.delete(Uf2_guess_f2, 0, axis=0)
        pre_sq_Uc_f3 = np.delete(Uf3_guess_f3, 0, axis=0)
        
        pre_sq_Uz_f1 = np.delete(Uz_f_sq[0], 0, axis=0)
        pre_sq_Uz_f2 = np.delete(Uz_f_sq[1], 0, axis=0)
        pre_sq_Uz_f3 = np.delete(Uz_f_sq[2], 0, axis=0)
        
        J1_0 = 1e6
        J2_0 = 1e6
        J3_0 = 1e6
        tag = 1
        
        time_end_f = time()
        print('------------------------------------------------')
        print('F-EMPC time: '+ str(time_end_f - time_start_f))
        print('------------------------------------------------')
        print('Iteration number: '+ str(It_num_f[i_f]))
        print('------------------------------------------------')
        
        pass
    
    # Combine optimal inputs    
    Uck = np.array([Uf1k[0], Uf2k[0], Usk[0],
                    Uf3k[0], Usk[1], Usk[2], Uf1k[1]])
    Drk = D_r[i,:]
    
    # Simulation
    Ik = I_ode_r(x0 = X_int, p = vertcat(Uck, Uzk, Drk))
    xk = Ik['xf'].full().ravel()
    yk = out_ies_r(xk, Uck, Uzk, Drk).full().ravel()
    
    # Record data
    X[i+1,:] = xk
    Y[i+1,:] = yk
    U[i,:] = Uck
    Z[i,:] = Uzk
    
    X_int = xk
    
    print('------------------------------------------------')
    print('Simulation time: ' + str(i*Delta_r))
    print('------------------------------------------------')
    
    # Save data every 30 mins
    if i % (30*60/Delta_r) == 0:
        
        np.savetxt("P1_X.txt", X, fmt='%.10e', delimiter = ', ')
        np.savetxt("P1_Y.txt", Y, fmt='%.10e', delimiter = ', ')
        np.savetxt("P1_U.txt", U, fmt='%.10e', delimiter = ', ')
        np.savetxt("P1_Z.txt", Z, fmt='%.10e', delimiter = ', ')
        np.savetxt("P1_Xf_stp_s.txt", Xf_stp_s, fmt='%.10e', delimiter = ', ')
        np.savetxt("P1_Uf_stp_s.txt", Uf_stp_s, fmt='%.10e', delimiter = ', ')
        np.savetxt("P1_It_num_f.txt", It_num_f, fmt='%.10e', delimiter = ', ')
    
    pass

# Add initlial/last points for Y, U, Z
Y[0,:] = Y[1,:]
U[-1,:] = U[-2,:]
Z[-1,:] = Z[-2,:]

time_end_sim = time()
print('------------------------------------------------')
print('Dsitributed EMPC time: '+str(time_end_sim - time_start_sim))
print('------------------------------------------------')

# In[11] Save data

np.savetxt("P1_X.txt", X, fmt='%.10e', delimiter = ', ')
np.savetxt("P1_Y.txt", Y, fmt='%.10e', delimiter = ', ')
np.savetxt("P1_U.txt", U, fmt='%.10e', delimiter = ', ')
np.savetxt("P1_Z.txt", Z, fmt='%.10e', delimiter = ', ')
np.savetxt("P1_Xf_stp_s.txt", Xf_stp_s, fmt='%.10e', delimiter = ', ')
np.savetxt("P1_Uf_stp_s.txt", Uf_stp_s, fmt='%.10e', delimiter = ', ')
np.savetxt("P1_It_num_f.txt", It_num_f, fmt='%.10e', delimiter = ', ')

